/////////////////////////////////////////
//yulishuai
//v1.0
//实现遥控器，ｃｍｄ＿ｖｅｌ转速控制，左肩键低速，右肩键告诉，ｘ遥控，ａ规划
//待改：ｊｏｙＦｌａｇ的初始值
/////////////////////////////////////////


#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<iostream>
#include "std_msgs/Int8.h"
#include <std_msgs/Float64.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Float64MultiArray.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

float WHEEL_L=2.8;
float WHEEL_R=0.6;
float WHEEL_PI=3.141693;
float v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
float cv1=0,cv2=0,cv3=0,cv4=0,cv5=0,cv6=0;
float speed_k=10;
int joyFlag;
double speed_x,speed_y,speed_w;

void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{

  float speed_x,speed_y,speed_w;
  geometry_msgs::Twist twist;
  double l_scale_=2, a_scale_=2; 

  // take the data from the joystick and manipulate it by scaling it and using independent axes to control the linear and angular velocities of the turtle
if(joy->buttons[2])		//X键：手柄［２］
{
	if(joy->buttons[5])
	{	
	twist.angular.z = a_scale_*joy->axes[0];//手柄axes[0]
	twist.linear.x = l_scale_*joy->axes[1]*speed_k;//手柄axes[1]
	}
	else if(joy->buttons[4]){
	twist.angular.z = a_scale_*joy->axes[0];//手柄axes[0]
	twist.linear.x = l_scale_*joy->axes[1];//手柄axes[1]
	}
	joyFlag=1;
	ROS_INFO("Received a /joy message!");
	speed_x = twist.linear.x;//v
	speed_y = 0;
	speed_w = twist.angular.z;//w

 	v1 =speed_x+0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
 	v3 =v1;
 	v5 =v1;
   
 	v2 =speed_x-0.5*WHEEL_L*speed_w;
 	v4 =v2;
 	v6 =v2;
  
 	v1 =v1/(2.0*WHEEL_R*WHEEL_PI);    //转换为轮子的速度　RPM
  	v2 =v2/(2.0*WHEEL_R*WHEEL_PI);
  	v3 =v3/(2.0*WHEEL_R*WHEEL_PI);
  	v4 =v4/(2.0*WHEEL_R*WHEEL_PI);
 	v5 =v5/(2.0*WHEEL_R*WHEEL_PI);
 	v6 =v6/(2.0*WHEEL_R*WHEEL_PI);

}
else if(joy->buttons[0])//手柄［０］
{
joyFlag=0;
ROS_INFO("justice joyFlag=0");
} 

}


void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{

  //float speed_x,speed_y,speed_w;
  

  ROS_INFO("Received a /cmd_vel message!");
  speed_x = msg->linear.x;//v
  speed_y = 0;
  speed_w = msg->angular.z;//w
  

   cv1 =speed_x-0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
   cv3 =cv1;
   cv5 =cv1;
   
   cv2 =speed_x+0.5*WHEEL_L*speed_w;
   cv4 =cv2;
   cv6 =cv2;
  
   cv1 =cv1/WHEEL_R;    //转换为轮子的速度　rad/s
   cv2 =cv2/WHEEL_R;
   cv3 =cv3/WHEEL_R;
   cv4 =cv4/WHEEL_R;
   cv5 =cv5/WHEEL_R;
   cv6 =cv6/WHEEL_R;
  

  
 

}
 
int main(int argc, char** argv)
{
	
	ros::init(argc, argv, "motive_control");
	ros::NodeHandle n;
	ros::Subscriber tsub = n.subscribe("/joy", 1000, joyCallback);
        ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_callback);
	ros::Publisher six_wheel_torque_pub = n.advertise<std_msgs::Float64MultiArray>("wheel_torque_publisher", 1000);//发布话题wheel_speed_publisher
        //ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
	
	
	//double x = 0.0;
	//double y = 0.0;
	//double th = 0.0;	
	
	//ros::Time current_time, last_time;
	//current_time = ros::Time::now();
 	//last_time = ros::Time::now();

 	ros::Rate loop_rate(10);

while (ros::ok())
  {
    std_msgs::Float64MultiArray msg;
    //current_time = ros::Time::now();
    //double dt = (current_time - last_time).toSec();
    //double delta_x = (speed_x * cos(th) - speed_y* sin(th)) * dt;
    //double delta_y = (speed_x * sin(th) + speed_y * cos(th)) * dt;
    //double delta_th = speed_w * dt;

   // x += delta_x;
   // y += delta_y;
   // th += delta_th;

    //geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);


    //nav_msgs::Odometry odom;
    //odom.header.stamp = current_time;
   // odom.header.frame_id = "odom";

    //odom.pose.pose.position.x = x;
    //odom.pose.pose.position.y = y;
    //odom.pose.pose.position.z = 0.0;
    //odom.pose.pose.orientation = odom_quat;
   // odom.child_frame_id = "base_link";
   // odom.twist.twist.linear.x = speed_x;
    //odom.twist.twist.linear.y = speed_y;
   // odom.twist.twist.angular.z = speed_w;

    //publish the message
   // odom_pub.publish(odom);

   // last_time = current_time;
    

if(joyFlag == 1)
{
msg.data.push_back(v1);
msg.data.push_back(v2);
msg.data.push_back(v3);
msg.data.push_back(v4);
msg.data.push_back(v5);
msg.data.push_back(v6);
//joyFlag=2;

    ROS_INFO("joyFlag=1");

}
else if(joyFlag == 0)
{
msg.data.push_back(cv1);
msg.data.push_back(cv2);
msg.data.push_back(cv3);
msg.data.push_back(cv4);
msg.data.push_back(cv5);
msg.data.push_back(cv6);
//joyFlag=2;

    ROS_INFO("joyFlag=0");

}

six_wheel_torque_pub.publish(msg);

    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;
}

